close all; clear; clc;

a2 = 0.05; a3 = 1.0; a4 = 0.05;
d1 = 0.5; d4 = 1.0; d6 = 0.1;

Alpha = [0, pi/2, 0, pi/2, -pi/2, pi/2];
A = [0, a2, a3, a4, 0, 0];
D = [d1, 0, 0, d4, 0, d6];
Theta = [0, pi/2, 0, 0, 0, 0];

DH_para.Alpha = Alpha;
DH_para.A = A;
DH_para.D = D;
DH_para.Theta = Theta;

for i = 1:6
    L(i) = Link('alpha', Alpha(i),'a', A(i),'d',D(i));L(i).offset = Theta(i); L(i).qlim=[-2 * pi, 2 * pi];L(i).mdh = 1;
end
robot = SerialLink(L, 'name', 'robot');

q0 = zeros(1, 6);
p_start = [1.5; -0.5; 0.9];
p_end = [0.5; 0.5; 0.1];
T_start = rt2tr(rotx(pi), p_start);
T_end = rt2tr(rotx(pi), p_end);

qe_start = IKine_six(T_start, q0, DH_para);
qe_end = IKine_six(T_end, q0, DH_para);
qi = qe_start;
Ti = FKine_six(qi, DH_para);
p_i = Ti(1:3, 4);

eps = 1e-3;

p_ob1 = [1.0; -0.2; 0.3; 0.05];
p_ob2 = [0.8; 0.05; 0.6; 0.05];

k_rad = 8;
k_rep = 0.02;
k_attr = 0.01;

i = 0;
qs = [qi'];
ps = [p_i'];
while norm(p_i - p_end) > eps
    [line1, line2, line3] = cal_lines_six(qi, DH_para);
    
    [~, p1, t1] = point_to_lineSegment(p_ob1, line1);
    [~, p2, t2] = point_to_lineSegment(p_ob1, line2);
    [~, p3, t3] = point_to_lineSegment(p_ob1, line3);
    
    F1_1 = repulsive_force(p_ob1, p1, k_rad, k_rep);
    F2_1 = repulsive_force(p_ob1, p2, k_rad, k_rep);
    F3_1 = repulsive_force(p_ob1, p3, k_rad, k_rep);

    [~, J1_1, J2_1, J3_1] = cal_Jacobian_six(qi, t1, t2, t3, DH_para);

    [~, p1, t1] = point_to_lineSegment(p_ob2, line1);
    [~, p2, t2] = point_to_lineSegment(p_ob2, line2);
    [~, p3, t3] = point_to_lineSegment(p_ob2, line3);
    
    F1_2 = repulsive_force(p_ob2, p1, k_rad, k_rep);
    F2_2 = repulsive_force(p_ob2, p2, k_rad, k_rep);
    F3_2 = repulsive_force(p_ob2, p3, k_rad, k_rep);
    
    [J, J1_2, J2_2, J3_2] = cal_Jacobian_six(qi, t1, t2, t3, DH_para);
    
    F4 = attractive_force(p_i, p_end, k_attr);

    delta1_1 = pinv(J1_1) * F1_1;
    delta2_1 = pinv(J2_1) * F2_1;
    delta3_1 = pinv(J3_1) * F3_1;
    delta1_2 = pinv(J1_2) * F1_2;
    delta2_2 = pinv(J2_2) * F2_2;
    delta3_2 = pinv(J3_2) * F3_2;

    pe = p_i + F4;
    Te = rt2tr(rotx(pi), pe);
    qi = IKine_six(Te, qi, DH_para);
    qi = qi + delta1_1 + delta2_1 + delta3_1 + delta1_2 + delta2_2 + delta3_2;

    Ti = FKine_six(qi, DH_para);
    p_i = Ti(1:3, 4);
    ps = [ps; p_i'];
    qs = [qs; qi'];
    
    i = i + 1;
    if i > 10000
        disp("planning failed");
        break;
    end
end

figure(1);
plot3(ps(:, 1), ps(:, 2), ps(:, 3));
plot_sphere(p_start, 0.05, 'g');
plot_sphere(p_end, 0.05, 'r');
plot_sphere(p_ob1(1:3), p_ob1(4));
plot_sphere(p_ob2(1:3), p_ob2(4));
robot.plot(qs);
